/*
mpc_v2m 可以作为普通的 mpc_controller 使用，也可以与 mpc_v2s 配合使用
*/

# pragma once

# include "mpc_controller.h"
# include "dataStruct.h"

class mpc_v2m : public mpc_controller
{
private:
    cpp_tools::threadPool m_threadPool;

    std::string UART_device = "/tmp/V1";
    int UART_bauds = 921600;

    int frequency = 1e4;

    double delta = 0;
    double F_x = 0;

    bool isRunning = true;
public:
    mpc_v2m()
        :mpc_controller()
    {
        cpp_tools::XmlParameterManager m_xml("device", host_path + settings_path + "/device/settings.xml");
        bool writeXML = false;
        if (!m_xml.getParameter("mpc_v2m", "fileName", UART_device))
        {
            std::cout << "mpc_v2m设备路径使用使用默认值: " << UART_device << std::endl;
            m_xml.setParameter("mpc_v2m", "fileName", UART_device);

            writeXML = true;
        }
        if (!m_xml.getParameter("mpc_v2m", "bauds", UART_bauds))
        {
            std::cout << "mpc_v2m波特率使用使用默认值: " << UART_bauds << std::endl;
            m_xml.setParameter("mpc_v2m", "bauds", UART_bauds);

            writeXML = true;
        }
        if (writeXML)
        {
            m_xml.saveToFile(host_path + settings_path + "/device/settings.xml");
        }

        std::thread m_thread0([&]() -> void {
            cpp_tools::UartVectorPort<double> m_uart(UART_device, UART_bauds);
            m_uart.clearBuffer();

            while (isRunning && m_uart.connection())
            {
                Eigen::VectorX<double> data;
                if (m_uart.receiveVector(data) > 0)
                {
                    if (data.size() == 0)
                    {
                        continue;
                    }
                    
                    double order = data(0);

                    if (order == static_cast<double>(data::communicate::lower_solver::mpc_v2_order::update))
                    {
                        if (data.size() < 11)
                        {
                            continue;
                        }

                        auto state = data.middleRows(1,10);
                        
                        update(state, delta, F_x, cpp_tools::getCurrentTimeAsDouble());
                    }
                    else if (order == static_cast<double>(data::communicate::lower_solver::mpc_v2_order::getControl))
                    {
                        Eigen::VectorXd state(10); // state 没有实际意义

                        auto [delta_new,F_x_new] = getControl(state, delta, F_x, cpp_tools::getCurrentTimeAsDouble());

                        Eigen::VectorX<double> message(3);
                        message << order, delta_new, F_x_new;
                        m_uart.sendVector(message);

                        delta = delta_new;
                        F_x = F_x_new;
                    }
                    else if (order == static_cast<double>(data::communicate::lower_solver::mpc_v2_order::refresh))
                    {
                        int info = refresh();

                        delta = 0;
                        F_x = 0;

                        Eigen::VectorX<double> message(2);
                        message << order, info;
                        m_uart.sendVector(message);
                    }
                }

                std::this_thread::sleep_for(std::chrono::microseconds(std::max((int)(1e6/frequency),1)));
            }
        });
        m_threadPool.push_back(m_thread0);
    };

    ~mpc_v2m()
    {
        isRunning = false;

        m_threadPool.joinAll();

        shutdown();
    };
};
